//
// Created by 刘昱杉2020 on 2023/4/14.
//

#ifndef CAR_MOTOR_H
#define CAR_MOTOR_H

#include "main.h"
#include "pid.h"

//#define  MOTOR_DEBUG
#define  MOTOR_NUM  2
#define ABS(x) ((x)>0?(x):-(x))
typedef  struct
{
    int16_t Angle;
    float TargetSpeed;
    float Speed;
    float TargetAngle;
    int32_t LastAngle;
    int32_t TotalAngle;
    PID   SpeedPID;
    CascadePID AnglePID;

}Motor_TT;

typedef struct {
    float TargetAngle;
    uint8_t id;
    uint8_t is_digital; //是否为数字舵机标志
}Servo;

typedef struct{
    Motor_TT motorTt[2];
    Servo digital_servo; //爪子的数字舵机
    Servo bus_servo[3]; //其余自由度的总线舵机

}Motor;

#ifdef   MOTOR_DEBUG
//统计马达的角度和速度
void Motor_motorTt_Cal(Motor *motor);
#endif

/********************外部调用函数*************************/
//设置马达pwm
void Motor_motorTt_SetPWM(float PWM1,float PWM2);

//获取马达速度角度
void Motor_motorTt_GetSpeed(Motor_TT *motor);
//开始计算电机累计角度
void Motor_motorTt_StartCalcAngle(Motor_TT *motor);

//舵机
uint8_t Motor_digital_servo_setAngle(Servo *servo,uint16_t angle);
uint8_t Motor_bus_servo_setID(uint8_t id);
uint8_t Motor_bus_servo_setAngle(Servo *servo,uint8_t id,uint16_t angle);

#endif //CAR_MOTOR_H
